
import time
import _thread
import ustruct as struct
from machine import SoftI2C, Pin, SPI, UART, PWM, Timer, ADC

from MPU6050 import MPU6050
import sx1280

from pid import PID

dpsCounter = 0

def buildRadio():
    CS = Pin(5, Pin.OUT, value=1)
    RESET = Pin(6, Pin.OUT, value=1)
    BUSY = Pin(7, Pin.IN, pull=None)
    RESET.value(0)
    RESET.value(1)

    spi = SPI(0, baudrate=10000000,polarity=0,phase=0,bits=8,
            firstbit=SPI.MSB, sck=Pin(2), mosi=Pin(3), miso=Pin(4))  # 实例化类 SoftSPI

    radio = sx1280.SX1280(spi, CS, RESET, BUSY, 2.48, debug=False)
    
    # Prepare radio for Rx
    radio.listen = True

    return radio

'''
def feedThread(self):
    global dpsCounter
    print("timer feed:%d"%dpsCounter)
    dpsCounter = 0

timer = Timer()         # global, single timer
timer.init(period=100, mode=Timer.PERIODIC, callback=feedThread)
'''

pitch = 0
roll = 0
yaw = 0

def run_on_core1():
    
    i2c = SoftI2C(sda=Pin(11), scl=Pin(10), freq=400_000)

    device_address = 0x68
    freq_divider = 0x04  # freq = 200Hz / (1 + value)  推荐 >=4

    # Make an MPU6050
    mpu = MPU6050(i2c, device_address, freq_divider)

    # Initiate your DMP
    mpu.dmp_initialize()
    mpu.set_DMP_enabled(True)

    packet_size = mpu.DMP_get_FIFO_packet_size()
    FIFO_buffer = [0]*64
    
    global pitch, roll, yaw
    
    while True:
            
        while not mpu.isreadyFIFO(packet_size): # Check if FIFO data are ready to use...
            pass
        
        FIFO_buffer = mpu.get_FIFO_bytes(packet_size) # get all the DMP data here
        
        qw,qx,qy,qz = mpu.DMP_get_quaternion_int16(FIFO_buffer)
        grav_x,grav_y,grav_z = mpu.DMP_get_gravity(qw,qx,qy,qz)
        roll, pitch, yaw = mpu.DMP_get_euler_roll_pitch_yaw(qw,qx,qy,qz)
        
        #print("roll: {}, pitch: {}, yaw: {}".format(roll, pitch, yaw))
        #print("calc thread read:%d"%dpsCounter)
        
def run_on_core0():
    
    #radio = buildRadio()

    pins = [
        {'pin': 12, 'freq': 100},
        {'pin': 13, 'freq': 100},
        {'pin': 14, 'freq': 100},
        {'pin': 15, 'freq': 100},
        {'pin': 16, 'freq': 100}
    ]
    for pitem in pins:
        pwm = PWM(Pin(pitem['pin'], Pin.OUT))
        pwm.freq(pitem['freq'])
        pitem['pwm'] = pwm

    adcVol = ADC(Pin(29))

    # PWM占空比控制
    freq = 100
    cycle = 1000 / freq

    dutyAll = 65535
    dutyStart = int(dutyAll * (0.5 / cycle))
    dutyEnd = int(dutyAll * (2.5 / cycle))
    dutyMiddle = int((dutyStart + dutyEnd) / 2)
    dutyRange = dutyEnd - dutyStart

    pins[3]['pwm'].duty_u16(dutyMiddle)
    pins[4]['pwm'].duty_u16(dutyMiddle)
                
    #range limit
    limitStart = int(dutyStart + dutyRange * 0.13)
    limitEnd = int(dutyEnd - dutyRange * 0.13)

    targetPitch = 0
    targetRoll = 0

    failTimes = 0

    rs_pid = PID(p=4, i=0.1, d=0.001)	# d:消除零界点的抖动
    rv_pid = PID(p=4, i=0.1, d=0.001)
    
    global pitch, roll, yaw, dpsCounter
    
    uart = UART(1, baudrate = 115200,bits = 8,parity = None,stop = 1,
             timeout=10, timeout_char=1, tx = Pin(4),rx = Pin(5))

    while True:
        
        ''' ###################################
        电池电压保护 9.6V 停止工作， =3.2 * 3
            ###################################'''
        batteryU16 = adcVol.read_u16()
        batteryVol = batteryU16 / 65535 * 3.3 * 6
        #print(batteryVol)
        if(batteryVol < 9.6 and batteryVol > 3.2):#不接电池有3V电压
            for i in range(0, len(pins)):
                pins[i]['pwm'].duty_u16(0)
            continue


        if uart.any():
            command = uart.readline()
            if not command:
                continue
            
            data = command.decode().strip()

            if( len(data) == 16):
                step = 4
                result = [int(data[i:i+step], 16) for i in range(0, len(data), step)]

                vals = list(result)
                '''
                for i in range(0, len(vals)):
                    pins[i]['pwm'].duty_u16(vals[i])
                '''
                (v0, v1,v2,v3) = vals
                pins[0]['pwm'].duty_u16(v0)
                #偏转角度不能大，大了会晃动
                targetRoll = (v2 - dutyMiddle)  / (dutyRange * 2.0) * (dutyRange / 25)
                targetPitch = (v3 - dutyMiddle) / (dutyRange * 2.0) * (dutyRange / 45)
                #print("v1-v5:{} {} {} {} {}".format(v1,v2,v3,v4,v5))
                #print("target roll:%f  target pitch:%f"%(targetRoll, targetPitch))
                #res = radio.send(struct.pack('H', batteryU16), header=False, keep_listening=True)
                #print(res)
            else:
                failTimes +=1
        else:
            failTimes +=1
        
        
        #多次没收到信号，关闭油门
        if failTimes > 300:
            failTimes = 0
            for i in range(0, len(pins)):
                pins[i]['pwm'].duty_u16(0)
            
            #targetPitch = 0
            #targetRoll = 0

        #PID 实测有偏置，后面再查，先手动修正，这个参数也可以作为遥控调节
        offsetRoll = 33
        offsetPitch = 35
        
        #targetRoll = 30
        
        adjustRoll = int((pitch + offsetRoll + targetRoll) * (dutyRange / 180))
        dutyRoll = rs_pid.get_pid(adjustRoll, 1)

        adjustPitch = int((roll + offsetPitch + targetPitch) * (dutyRange / 180))
        dutyPitch = rv_pid.get_pid(adjustPitch, 1)
        #dutyPitch = limitEnd - dutyPitch + limitStart
        
        #base offset
        dutyCurrentL = dutyRoll + (limitEnd - dutyPitch + limitStart - dutyMiddle)
        dutyCurrentR = dutyRoll + (dutyPitch - dutyMiddle)
        
        dutyCurrentL = int(max(limitStart, min(dutyCurrentL,limitEnd)))
        dutyCurrentR = int(max(limitStart, min(dutyCurrentR,limitEnd)))
        
        pins[3]['pwm'].duty_u16(dutyCurrentL)
        pins[4]['pwm'].duty_u16(dutyCurrentR)
        
        dpsCounter+=1

        #time.sleep_ms(20)
        
        #time.sleep(1)

#睡眠等待三秒以方便开发调试
time.sleep(3)
_dmpt = _thread.start_new_thread(run_on_core1, ())
run_on_core0()



